function  []=vrrobot( robot )
% VRROBOT Create virtual world for robot (axes) animation 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create wrl file of robot reference frame 0
file=sprintf([...
'#VRML V2.0 utf8                                                    \n',...
'#     Created with Robotics Symbolic Matlab Toolbox                \n',...
'#     Instituto Superior T?cnico - DEM                             \n',...
'Background {                                                       \n',...
'  skyColor 0.3125 0.4336 0.6328                                    \n',...
'  groundColor 0 0 0.63                                             \n',...
'}                                                                  \n',...
'WorldInfo {                                                        \n',...
'  title "D-H Reference Frames for Virtual Reality Robot"           \n',...
'  info ["Copyright 2011 DEM IST" "$Revision: 0 $" "$Date: 2011/05/31 $" "$Author: Jorge M. M. Martins $" ] \n',...
'}                                                                  \n',...
'NavigationInfo {                                                   \n',...
'  type "EXAMINE"                                                   \n',...
'}                                                                  \n',...
'Viewpoint {                                                        \n',...
'  description "View 1"                                             \n',...
'  orientation 1 0 0 1.5708                                         \n',...
'  position 0 -3 1.1                                                \n',...
'}                                                                  \n',...
'DEF Frame0 Transform {                                             \n',...
' children [                                                        \n',...
          vrmlaxes('x0')                                               ,...                    
          vrmlaxes('y0')                                               ,...
          vrmlaxes('z0')                                                ]);

         
%Close parenthesis and brackets at end of file
fileend=sprintf([']\n' '}\n']);
 

for i=1:size(robot,1)  %add all links
    if robot(i,1)==['q' num2str(i)]
        d=double(robot(i,5)); %get ref value if d is a coordinate
        v=double(robot(i,2));
    else
        d=double(robot(i,1));
        v=double(robot(i,5)); %get ref value if v is a coordinate
    end
    a=double(robot(i,3));
    alpha=double(robot(i,4));
    
    %Add link to file
    file=[file vrmllink(i,d,v,a,alpha)];
       
    %Add closing parenthesis and brackets at end of file
    fileend=sprintf([']\n' '}\n' '}\n' '}\n' fileend]);
    
end
file=[file fileend]; % Join file with endoffile
fid=fopen('vrrobot.wrl','w');
fprintf(fid,'%s',file);
fclose(fid);
          
        
    function link = vrmllink(linknumber,d,v,a,alpha)
            
    link=sprintf([...    
    ' DEF Jt', num2str(linknumber), '_DH_dv Transform {             \n',...
    ' translation 0 0 ', num2str(d), '                              \n',...
    ' rotation 0 0 1 ', num2str(v), '                               \n',...
    '  children DEF Jt', num2str(linknumber), '_DH_aa Transform {   \n',...
    '    translation ', num2str(a), ' 0 0                           \n',...   
    '    rotation 1 0 0 ', num2str(alpha), '                        \n',...
    '    children DEF Link', num2str(linknumber), ' Transform {     \n',...
    '      children [                                               \n',...
             vrmlaxes(['x' num2str(linknumber)])                       ,...                    
             vrmlaxes(['y' num2str(linknumber)])                       ,...
             vrmlaxes(['z' num2str(linknumber)])                       ]);


    function  axes = vrmlaxes(label)
            
        if label(1)=='x'
            transrot=sprintf(['translation .1 0 0        \n',...
                             'rotation 0 0 -1 1.5708   \n']);
        end
        if label(1)=='y'
            transrot=sprintf('translation 0 .1 0        \n');
        end
        if label(1)=='z'
            transrot=sprintf(['translation 0 0 .1       \n',...
                              'rotation 1 0 0 1.5708   \n']);
        end
        
        
            axes=sprintf([...
                '  DEF ', label ,'_axes Transform {                            \n',...
                                transrot                                          ,...
                '  children [                                                  \n',...
                '    DEF ', label,'_axes_arrowbody Shape {                     \n',...
                '      geometry Cylinder {                                     \n',...
                '        radius 0.005                                           \n',...
                '        height 0.2                                            \n',...
                '      }                                                       \n',...
                '      appearance Appearance {                                 \n',...
                '        material Material {                                   \n',...
                '        }                                                     \n',...
                '      }                                                       \n',...
                '    }                                                         \n',...
                '    DEF ', label, '_axes_arrowtip Transform {                 \n',...
                '     translation 0 .12 0                                      \n',...
                '     children Shape {                                         \n',...
                '        geometry Cone {                                       \n',...
                '          height .04                                          \n',...
                '          bottomRadius .015                                   \n',...
                '        }                                                     \n',...
                '        appearance Appearance {                               \n',...
                '          material Material {                                 \n',...
                '          }                                                   \n',...
                '        }                                                     \n',...
                '      }                                                       \n',...
                '    }                                                         \n',...
                '    DEF ', label, '_axes_label Transform {                    \n',...
                '      translation 0 .15 0                                     \n',...
                '      children Billboard {                                    \n',...
                '        axisOfRotation 0 0 0                                  \n',...
                '        children Shape {                                      \n',...
                '          geometry DEF TEXT Text {                            \n',...
                '            length 0                                          \n',...
                '            fontStyle FontStyle {                             \n',...
                '              style "BOLDITALIC"                              \n',...
                '              size 0.1                                        \n',...
                '              family "SANS"                                   \n',...
                '            }                                                 \n',...
                '            string "', label,'"                               \n',...
                '          }                                                   \n',...
                '        }                                                     \n',...
                '      }                                                       \n',...
                '    }                                                         \n',...
                '  ]                                                           \n',...
                '}                                                             \n']);
    









